// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and
// the qglviewer library : http://www.libqglviewer.com


#ifndef _OpenMeca_Geom_Vector_hpp_
#define _OpenMeca_Geom_Vector_hpp_

#include <vector>

#include <iostream>
#include <cassert>
#include <string>
#include <cmath>

#include "OpenMeca/Geom/SpaceDim.hpp"
#include "OpenMeca/Geom/Coordinate.hpp"


#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/function.hpp>

//
// This is the Free Vector in a dimension N workspace.
//

namespace OpenMeca
{

  namespace Geom
  {

    //*************************
    // EXTERN OPERATOR

    template<SpaceDim N>
    std::ostream& operator<< (std::ostream &, const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator+ (const Vector<N> &, const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator- (const Vector<N> &, const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator- (const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator* (const Vector<N> &, const double &);

    template<SpaceDim N>
    Vector<N> operator* (const double &, const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator/ (const Vector<N> &, const double &);

    template<SpaceDim N>
    double   operator* (const Vector<N> &, const Vector<N> &);

    template<SpaceDim N>
    Vector<N> operator* (const Vector<N> &, const typename Vector<N>::DoubleArray &);

    template<SpaceDim N>
    Vector<N> operator^ (const Vector<N> &, const Vector<N> &);

    template<SpaceDim N>
    bool     operator!= (const Vector<N> &, const Vector<N> &);

    template<SpaceDim N>
    bool     operator== (const Vector<N> &, const Vector<N> &);


    template<SpaceDim N> class Point;

    template<SpaceDim N>
    class Vector
    {

    public: // Data
      
      static std::string GetStrKey(){return std::string("Vector" + SpaceDimUtil<N>::GetStrKey());}

      typedef double DoubleArray[N];

      friend class Quaternion<N>;
      //Vector Operator
      template<SpaceDim M> friend std::ostream& operator<< (std::ostream &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator+ (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator- (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator- (const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator* (const Vector<M> &, const double &);
      template<SpaceDim M> friend Vector<M> operator* (const double &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator/ (const Vector<M> &, const double &);
      template<SpaceDim M> friend double   operator* (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator* (const Vector<M> &, const typename Vector<M>::DoubleArray &);
      template<SpaceDim M> friend Vector<M> operator^ (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend bool     operator!= (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend bool     operator== (const Vector<M> &, const Vector<M> &);
      //Quaternion Operator
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const Vector<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Vector<M> &, const Quaternion<M> &);
      //Matrix Operator
      friend Vector<_3D> operator*(const Matrix<_3D>&, const Vector<_3D> &);

    public: // Methods
      explicit Vector (boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Vector (const Vector<N> &);
      explicit Vector (const Vector<N> &, boost::function<const Frame<N>& ()>);
      explicit Vector (const Coordinate<N, Cartesian> &);
      explicit Vector (const Point<N> &);
      explicit Vector (const Point<N> &, const Point<N>&);
      Vector (double x, double y, double z, boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      virtual ~Vector();

      // CAST TO POINT
      Point<N> & GetPoint();
      const Point<N> & GetPoint() const;

      Vector & operator= (const Vector &);

      SpaceDim GetDimension() const ;
      const Frame<N> & GetFrame() const;

      Coordinate<N, Cartesian> & GetCoordinate();
      const Coordinate<N, Cartesian> & GetCoordinate() const;

      void Clear();

      void Set(const Point<N> &,const Point<N> &);
      void Minus(const Vector<N> &,const Vector<N> &);

      std::ostream & Write (std::ostream &) const;
      std::ostream & XmlWrite (std::ostream &) const;

      // OPERATOR
      void operator+= (const Vector &);
      void operator-= (const Vector &);
      void operator*= (double k);
      void operator/= (double k);

      const double & operator[] (int i) const;
      double& operator[] (int i);

      // BOOLEAN ALGEBRA
      bool IsNull() const;

      // NORLMALIZING OPERATION
      double GetSquaredNorm() const;
      double GetNorm() const;
      double GetNorm1() const;
      double Normalize();
      Vector Unit() const;

      // PROJECTION
      void ProjectOnAxis (const Vector & direction);
      void ProjectOnPlane (const Vector & normal);
      Vector GetOrthogonalVector() const;

      // Changing Frame
      Vector ToGlobalFrame() const;
      Vector ToLocalFrame(const Frame<N>&) const;

      void ToGlobalFrame(Vector<N>&) const;
      void ToLocalFrame(const Frame<N>&, Vector<N>&) const;
      
      boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() {return coord_.GetFrameFunctionAccess();}
      const boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() const {return coord_.GetFrameFunctionAccess();}


    private:
      Coordinate<N, Cartesian> coord_;

      //BOOST SERIALIZATION       
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive & ar , const unsigned int ) 
      {
	ar & BOOST_SERIALIZATION_NVP( coord_);
      }

    };

    //
    // Definition of static generic template data must be in header file:
    //

    

    //
    // Vorbidden non-specialized methods (should be specialized)
    //

    //
    // Template methods
    //

    // Constructors ...

    template<SpaceDim N> inline
    Vector<N>::Vector (boost::function<const Frame<N>& ()> f)
        : coord_ (f)
    {
    }

    template<SpaceDim N> inline
    Vector<N>::Vector (const Vector<N> & v)
        : coord_ (v.coord_)
    {
    }

    template<SpaceDim N> inline
    Vector<N>::Vector (const Vector<N> & v, boost::function<const Frame<N>& ()> f)
        :  coord_ (f)
    {
      if (v.GetFrame()==f())
	{
	  *this=v;
	}

      else if (v.GetFrame().GetReferenceFrame()==f())
	{
	  *this  = v.GetFrame().GetQuaternion().Rotate(v);
	}

      else if (v.GetFrame()==f().GetReferenceFrame())
	{
	  *this  = f().GetQuaternion().InverseRotate(v);
	}
      
      else 
	{
	  Quaternion<N> q(v.GetFrame().GetQuaternion(),f);
	  //q.Rotate(v,*this); //TODO::Debug this function
	  *this = q.Rotate(v);
	}
    }


    template<SpaceDim N> inline
    Vector<N>::Vector (const Point<N> & p)
        : coord_ (p.GetCoordinate())
    {
      assert (typeid (typename Point<N>::myCoordSystem) == typeid (Cartesian));
    }

    template<SpaceDim N> inline
    Vector<N>::Vector (const Point<N> & p1, const Point<N> & p2)
      : coord_ (p2.coord_ - p1.coord_)
    {
       assert (typeid (typename Coordinate<N, Cartesian>::myCoordSystem) == typeid (Cartesian));
    }

    template<SpaceDim N> inline
    Vector<N>::~Vector ()
    {
    }


    // Conversions ...

    template<SpaceDim N> inline
    Point<N> &
    Vector<N>::GetPoint()
    {
      return *reinterpret_cast<Point<N> *> (this);
    }

    template<SpaceDim N> inline
    const Point<N> &
    Vector<N>::GetPoint() const
    {
      return *reinterpret_cast<const Point<N> *> (this);
    }

    // Accessors ...

    template<SpaceDim N> inline
    SpaceDim
    Vector<N>::GetDimension() const
    {
      return coord_.dimension;
    }

    template<SpaceDim N> inline
    const Frame<N> &
    Vector<N>::GetFrame() const
    {
      return coord_.GetFrame();
    }

    template<SpaceDim N> inline
    Coordinate<N, Cartesian> &
    Vector<N>::GetCoordinate()
    {
      return coord_;
    }

    template<SpaceDim N> inline
    const Coordinate<N, Cartesian> &
    Vector<N>::GetCoordinate() const
    {
      return coord_;
    }

    template <SpaceDim N> inline
    const double &
    Vector<N>::operator[] (int i) const
    {
      return coord_.c_[i];
    }

    template <SpaceDim N> inline
    double &
    Vector<N>::operator[] (int i)
    {
      return coord_.c_[i];
    }

    // External Operators ...

    template<SpaceDim N> inline
    bool
    operator== (const Vector<N> &a, const Vector<N> &b)
    {
      return (a -b).GetSquaredNorm() < Vector<N>::epsilon;
    }

    template<SpaceDim N> inline
    bool
    operator!= (const Vector<N> &a, const Vector<N> &b)
    {
      return ! (a == b);
    }

    template<SpaceDim N> inline
    std::ostream&
    operator<< (std::ostream &out, const Vector<N> &v)
    {
      std::cout.setf(std::ios::scientific, std::ios::floatfield);
      switch (N)
      {
        case _3D : return out << v.coord_[0] << "\t" << v.coord_[1] << "\t"  << v.coord_[2] <<"\t" ; break;
        case _2D : return out << v.coord_[0] << "\t"  << v.coord_[1]  << "\t" ; break;
        case _1D : return out << v.coord_[0]  << "\t" ; break;
        case _0D : return out << "\t" ; break;
        default : assert(0); break;
	}
    }


    template<SpaceDim N> inline
    Vector<N>
    operator* (const double & k, const Vector<N> &v)
    {
      return v*k;
    }


    template<SpaceDim N> inline
    double
    operator* (const Vector<N> &a, const Vector<N> &b)
    {
      assert (a.GetFrame() == b.GetFrame());
      double sum = 0;

      for (int i = 0; i < N; ++i)
      {
        sum += a.coord_.c_[i] * b.coord_.c_[i];
      }

      return sum;
    }

    template<SpaceDim N> inline
    Vector<N>
    operator* (const Vector<N> &a, const typename Vector<N>::DoubleArray & b)
    {
      Vector<N> v(a);
      for (int i = 0; i < N; ++i)
      {
        v.coord_.c_[i] *= b[i];
      }
      return v;
    }

    template<SpaceDim N> inline
    double
    GetAngleBetweenUnitVectors (const Vector<N> &a, const Vector<N> &b)
    {
      return acos(a*b);
    }
    
    // Class Operators ...

    template<SpaceDim N> inline
    Vector<N> &
    Vector<N>::operator= (const Vector<N> & v)
    {
      coord_ = v.coord_;
      return *this;
    }

    template<SpaceDim N> inline
    void
    Vector<N>::operator+= (const Vector<N> & a)
    {
      coord_ += a.coord_;
    }

    template<SpaceDim N> inline
    void
    Vector<N>::operator-= (const Vector<N> & a)
    {
      coord_ -= a.coord_;
    }

    template<SpaceDim N> inline
    void
    Vector<N>::operator*= (double k)
    {
      coord_ *= k;
    }

    template<SpaceDim N> inline
    void
    Vector<N>::operator/= (double k)
    {
      coord_ /= k;
    }

    template<SpaceDim N> inline
    double
    Vector<N>::GetSquaredNorm() const
    {
      double sum = 0;

      for (int i = 0; i < N; ++i)
      {
        sum += coord_.c_[i] * coord_.c_[i];
      }

      return sum;
    }

    template<SpaceDim N> inline
    double
    Vector<N>::GetNorm() const
    {
      return sqrt (GetSquaredNorm());
    }

    template<SpaceDim N> inline
    double
    Vector<N>::GetNorm1() const
    {
      double sum = 0;

      for (int i = 0; i < N; ++i)
      {
        sum += fabs(coord_.c_[i]);
      }
      return sum;
    }

    // Other Methods

    template<SpaceDim N> inline
    void
    Vector<N>::Clear()
    {
      for (int i = 0; i < N; ++i)
      {
        coord_.c_[i]=0.;
      }
    }

    template<SpaceDim N> inline
    void
    Vector<N>::Set(const Point<N> &p1, const Point<N> &p2)
    {
      coord_  = p2.coord_;
      coord_ -= p1.coord_;
    }
    
    template<SpaceDim N> inline
    void
    Vector<N>::Minus(const Vector<N> &v1, const Vector<N> &v2)
    {
      coord_  = v1.coord_;
      coord_ -= v2.coord_;
    }

    template <SpaceDim N> inline
    bool
    Vector<N>::IsNull() const
    {
      return (GetSquaredNorm() < Vector<N>::epsilon);
    }

    template<SpaceDim N> inline
    Vector<N>
    Vector<N>::Unit() const
    {
      Vector<N> v (*this);
      v.Normalize();
      return v;
    }

    template <SpaceDim N>  inline 
    Vector<N> 
    Vector<N>::ToGlobalFrame() const
    {
      //Return expression of this vector in the globalFrame.
      //Available only if the rank of the vector is 1.
      //Usefull to boost computation
      assert (GetFrame().GetRank()==1);
      return GetFrame().GetQuaternion().Rotate(*this);
    }

    template <SpaceDim N> inline 
    Vector<N> 
    Vector<N>::ToLocalFrame(const Frame<N>& f) const
    {
      //Return expression of this vector in local frame.
      //Available only if the rank of the vector is 0 (express in GlobalFrame) and localFrame is 1.
      //Usefull to boost computation
      assert (GetFrame().GetRank()==0 && f.GetRank()==1);
      return f.GetQuaternion().InverseRotate(*this);
    }
    

    template <SpaceDim N> inline
    void 
    Vector<N>::ToGlobalFrame(Vector<N>& v_out) const
    {
      //Return expression of this vector in local frame in v_out.
      //Similar to "ToGlobalFrame()" method without temporary copy (faster)
      //Usefull to boost computation
      assert (GetFrame().GetRank()==1);
      GetFrame().GetQuaternion().Rotate(*this, v_out);
    }
      
    template <SpaceDim N> inline
    void
    Vector<N>::ToLocalFrame(const Frame<N>& f, Vector<N>& v_out) const
    {
      //Return expression of this vector in local frame.
      //Similar to "ToLocalFrame(const Frame<N>&)" method without temporary copy (faster)
      //Usefull to boost computation
      assert (GetFrame().GetRank()==0 && f.GetRank()==1);
      f.GetQuaternion().InverseRotate(*this, v_out);
    }

    template<SpaceDim N> inline 
    std::ostream &
    Vector<N>::Write (std::ostream & out) const
    {
      std::cout.setf (std::ios::scientific, std::ios::floatfield);

      switch (N)
      {

      case _3D :
        return out << "Vector<_3D> (" << coord_.c_[0] << ", " << coord_.c_[1] << ", " << coord_.c_[2] << ")";
        break;

      case _2D :
        return out << "Vector<_2D> (" << coord_.c_[0] << ", " << coord_.c_[1]  << ")";
        break;

      case _1D :
        return out << "Vector<_1D> (" << coord_.c_[0]  << ")";
        break;

      case _0D :
        return out << "Vector<_0D> ()";
        break;

      default :
        assert (0);
        break;
      }
    }

    template<SpaceDim N> inline 
    std::ostream &
    Vector<N>::XmlWrite (std::ostream & out) const
    {
      std::cout.setf (std::ios::scientific, std::ios::floatfield);
      out << "<" << GetStrKey() << "> " ;
      coord_.XmlWrite (out);
      out << "</" << GetStrKey() << "> " ;
      return out;
    }

    ///////////////////////////////////
    // template specialisation : _3D //
    ///////////////////////////////////

    // Constructors ...
    

    template<> inline
    Vector<_3D>::Vector (double x, double y, double z, boost::function<const Frame<_3D>& ()> f)
        : coord_ (x, y, z, f)
    {
    }

    // Accessors ...

    // External Operators ...

    template<> inline
    Vector<_3D>
    operator+ (const Vector<_3D> &a, const Vector<_3D> &b)
    {
      assert (a.GetFrame() == b.GetFrame());
      return Vector<_3D> (a.coord_.c_[0] + b.coord_.c_[0], a.coord_.c_[1] + b.coord_.c_[1], a.coord_.c_[2] + b.coord_.c_[2], a.GetFrameFunctionAccess());
    }

    template<> inline
    Vector<_3D>
    operator- (const Vector<_3D> &a, const Vector<_3D> &b)
    {
      assert (a.GetFrame() == b.GetFrame());
      return Vector<_3D> (a.coord_.c_[0] - b.coord_.c_[0], a.coord_.c_[1] - b.coord_.c_[1], a.coord_.c_[2] - b.coord_.c_[2], a.GetFrameFunctionAccess());
    }

    template<> inline
    Vector<_3D>
    operator- (const Vector<_3D> &a)
    {
      return Vector<_3D> (-a.coord_.c_[0], -a.coord_.c_[1], -a.coord_.c_[2], a.GetFrameFunctionAccess());
    }

    template<> inline
    Vector<_3D>
    operator* (const Vector<_3D> &a, const double &d)
    {
      return Vector<_3D> (d*a.coord_.c_[0], d*a.coord_.c_[1], d*a.coord_.c_[2], a.GetFrameFunctionAccess());
    }

    template<> inline
    Vector<_3D>
    operator/ (const Vector<_3D> &a, const double &k)
    {
      if (fabs (k) == 0.)
      {
        std::cerr << "operator/ : dividing a vector by a quasi-null value : " << k << std::endl << std::flush;
        assert (0);
        // TODO : generate an exception !!!
      }

      return Vector<_3D> (a.coord_.c_[0] / k, a.coord_.c_[1] / k, a.coord_.c_[2] / k, a.GetFrameFunctionAccess());
    }

    // Class Operators ...

    template<> inline
    double
    Vector<_3D>::GetNorm() const
    {
      return sqrt(coord_.c_[0]*coord_.c_[0] + coord_.c_[1]*coord_.c_[1] + coord_.c_[2]*coord_.c_[2]);
    }

    // Other Methods ...

    template<> inline
    Vector<_3D>
    operator^ (const Vector<_3D> &a, const Vector<_3D> &b)
    {
      assert (a.GetFrame() == b.GetFrame());
      return Vector<_3D> (a.coord_.c_[1]*b.coord_.c_[2] - a.coord_.c_[2]*b.coord_.c_[1],
                          a.coord_.c_[2]*b.coord_.c_[0] - a.coord_.c_[0]*b.coord_.c_[2],
                          a.coord_.c_[0]*b.coord_.c_[1] - a.coord_.c_[1]*b.coord_.c_[0],
                          a.GetFrameFunctionAccess());
    }

    template<> inline
    double
    Vector<_3D>::Normalize()
    {
      const double n = GetNorm();

      if (n == 0.)
      {
        // std::cerr << "vector3d::normalize: normalizing a null vector (norm=" << n << "), the vector stay null." << std::endl;
        coord_.c_[0] = 0.;
        coord_.c_[1] = 0.;
        coord_.c_[2] = 0.;
        return 0.;
      }
      else
      {
        *this /= n;
        return n;
      }
    }
    
    ///////////////////////////////////
    // template specialisation : _2D //
    ///////////////////////////////////

    // Accessors ...

    // External Operators ...

    // Class Operators ...

    // Other Methods ...

  }
}


#endif
